#include "sensors_MPU6050.h"
#include "driverLog.h"
#include "driverI2C_ISR.h"
#include "types.h"
#include "state.h"
#include "sensors.h"
#include "config.h"
#include "tools.h"
#include "main_peripherals.h"

    BOOL mpu6050Init();
    BOOL mpu6050SetRate();
    
// 14 bytes receive buffer for i2c (accx,accy,accz,temp,gyrox,gyroy,gyroz)
BYTE					mpuBuffer[14];

// i2c action triggered on interrupt (INT2)
struct i2cAction_t		mpu;

BYTE                                    mpuReadRegister;

/**
 * Callback from I2C ISR > data received
 *
 * !! Should not do lots of stuff !!
 */
void mpu6050Callback(portBASE_TYPE * const higherPrior)
{
	if (mpu.state != I2C_STATE_DONE)
	{
		STATE.mpu6050.failures++;
		//STATE.mpu6050.connected = 0;
		return;
	}

	STATE.mpu6050.totalSamples++;
    
	STATE.mpu6050.bufferGyro[STATE.mpu6050.bufferGyroPosition] = (SHORT3){
		IMU_GYRO_AXIS_X_SIGN(short)((mpuBuffer[IMU_GYRO_AXIS_X] << 8) | mpuBuffer[IMU_GYRO_AXIS_X+1]),
		IMU_GYRO_AXIS_Y_SIGN(short)((mpuBuffer[IMU_GYRO_AXIS_Y] << 8) | mpuBuffer[IMU_GYRO_AXIS_Y+1]),
		IMU_GYRO_AXIS_Z_SIGN(short)((mpuBuffer[IMU_GYRO_AXIS_Z] << 8) | mpuBuffer[IMU_GYRO_AXIS_Z+1])
	};
	//logStoreShort3(xTaskGetTickCountFromISR(), LOG_ID_GYRO_RAW, STATE.mpu6050.bufferGyro[STATE.mpu6050.bufferGyroPosition]);
    logStore(xTaskGetTickCountFromISR(), LOG_ID_GYRO_RAW, STATE.mpu6050.bufferGyro[STATE.mpu6050.bufferGyroPosition]);

	STATE.mpu6050.bufferGyroPosition++;

	if (STATE.mpu6050.bufferGyroPosition == MPU_GYRO_BUFFERSIZE)
	{
		STATE.mpu6050.bufferGyroPosition = 0;

		if (STATE.mpu6050.gyroBufferFilled == 0)
		{
			STATE.mpu6050.gyroBias.x = STATE.mpu6050.gyroBias.y = STATE.mpu6050.gyroBias.z = 0;

			int i;
			for (i=0;i < MPU_GYRO_BUFFERSIZE; i++)
			{
				STATE.mpu6050.gyroBias.x += STATE.mpu6050.bufferGyro[i].x;
				STATE.mpu6050.gyroBias.y += STATE.mpu6050.bufferGyro[i].y;
				STATE.mpu6050.gyroBias.z += STATE.mpu6050.bufferGyro[i].z;
			}

			// TOOD: Overflow short?
			STATE.mpu6050.gyroBias.x /= MPU_GYRO_BUFFERSIZE;
			STATE.mpu6050.gyroBias.y /= MPU_GYRO_BUFFERSIZE;
			STATE.mpu6050.gyroBias.z /= MPU_GYRO_BUFFERSIZE;

			STATE.mpu6050.gyroBufferFilled = 1;
		}
	}

	STATE.mpu6050.bufferAcc[STATE.mpu6050.bufferAccPosition] = (SHORT3){
		IMU_ACC_AXIS_X_SIGN(short)((mpuBuffer[IMU_ACC_AXIS_X] << 8) | mpuBuffer[IMU_ACC_AXIS_X+1]),
		IMU_ACC_AXIS_Y_SIGN(short)((mpuBuffer[IMU_ACC_AXIS_Y] << 8) | mpuBuffer[IMU_ACC_AXIS_Y+1]),
		IMU_ACC_AXIS_Z_SIGN(short)((mpuBuffer[IMU_ACC_AXIS_Z] << 8) | mpuBuffer[IMU_ACC_AXIS_Z+1])
	};
    logStore(xTaskGetTickCountFromISR(), LOG_ID_ACC_RAW, STATE.mpu6050.bufferAcc[STATE.mpu6050.bufferAccPosition]);
	//logStoreShort3(xTaskGetTickCountFromISR(), LOG_ID_ACC_RAW, STATE.mpu6050.bufferAcc[STATE.mpu6050.bufferAccPosition]);

	STATE.mpu6050.bufferAccPosition++;
	if (STATE.mpu6050.bufferAccPosition == MPU_ACC_BUFFERSIZE)
	{
		STATE.mpu6050.bufferAccPosition = 0;
		STATE.mpu6050.accBufferFilled = 1;
	}
    
    extern SemaphoreHandle_t semStability;
    xSemaphoreGiveFromISR(semStability, higherPrior);
}

/**
 * Checks if MPU6050 who-am-i register is correct.
 *
 * @return BOOL true if succeeded
 */
BOOL mpu6050Check()
{
	struct i2cAction_t * result;
	// Who am I reg.
	result = sensorRead(MPU6050_ADDRESS,0x75,0x00);
	if (result->state == I2C_STATE_DONE && result->readPacket[0] == 0x68) return TRUE;
	return FALSE;
}

/**
 * Sets the power management; mpu must be activated
 *
 * @return BOOL true if succeeded
 */
BOOL mpu6050SetPowerManagement(BYTE pwr)
{
    struct i2cAction_t * result;

    // Power management 1 reg (reset,sleep,cycle-mode,temp_dis,clock_select)
    result = sensorWrite(MPU6050_ADDRESS,0x6B,pwr);
    if (result->state != I2C_STATE_DONE) return FALSE;
    return TRUE;
}

/**
 * Set the sample rate  and low pass filter
 *
 * @return BOOL true if succeeded
 */
BOOL mpu6050SetRate()
{
    struct i2cAction_t * result;
    int rate = CONFIG.mpu6050.hardwareFreq;

    result = sensorWrite(MPU6050_ADDRESS,0x1A,CONFIG.mpu6050.hardwareLPF & 0x07);
    if (result->state != I2C_STATE_DONE) return FALSE;

    if (CONFIG.mpu6050.hardwareLPF == 0) // 8khz samplerate
        rate = 8000 / (1 + rate);
    else
        rate = 1000 / (1 + rate);

    result = sensorWrite(MPU6050_ADDRESS,0x19,rate);
    if (result->state != I2C_STATE_DONE) return FALSE;

    return TRUE;
}

/**
 * Set the acc & gyro scales
 *
 * @return BOOL true if succeeded
 */
BOOL mpu6050SetSensorScales()
{
    struct i2cAction_t * result;

    // Gyro range [self test x, y, z, RANGE, RANGE, 0, 0, 0]
    result = sensorWrite(MPU6050_ADDRESS,0x1B,(CONFIG.mpu6050.hardwareGyroRange & 0x03) << 3);
    if (result->state != I2C_STATE_DONE) return FALSE;

    // Acc range [self test x, y, z, RANGE, RANGE, 0, 0, 0]
    result = sensorWrite(MPU6050_ADDRESS,0x1C,(CONFIG.mpu6050.hardwareAccRange & 0x03) << 3);
    if (result->state != I2C_STATE_DONE) return FALSE;

    return TRUE;
}


/**
 * Configures the mpu interrupt pin
 *
 * @return BOOL true if succeeded
 */
BOOL mpu6050SetInterrupts()
{
    struct i2cAction_t * result;

	#define MPU_INTCONF_ACTIVE_HIGH			0x00
	#define MPU_INTCONF_ACTIVE_LOW			0x80
	#define MPU_INTCONF_OPEN_PUSH_PULL		0x00
	#define MPU_INTCONF_OPEN_DRAIN			0x40
	#define MPU_INTCONF_LATCH_PULSE			0x00
	#define MPU_INTCONF_LATCH_HOLD			0x20
	#define MPU_INTCONF_CLEAR_INT			0x00
	#define MPU_INTCONF_CLEAR_READ			0x10

	#define MPU_INTON_DATA_READY			0x01

    result = sensorWrite(MPU6050_ADDRESS,0x37,MPU_INTCONF_ACTIVE_HIGH|MPU_INTCONF_CLEAR_READ|MPU_INTCONF_LATCH_PULSE|MPU_INTCONF_OPEN_PUSH_PULL);
    if (result->state != I2C_STATE_DONE) return FALSE;

    result = sensorWrite(MPU6050_ADDRESS,0x38,MPU_INTON_DATA_READY);
    if (result->state != I2C_STATE_DONE) return FALSE;

    return TRUE;
}

void mpu6050FreqChanged(const void * ref, const void * value)
{
    CONFIG.mpu6050.hardwareFreq = SHORT_FROM_BYTES_REV(((BYTE*)value), 0);
    mpu6050Reset();
}

void mpu6050LPFChanged(const void * ref, const void * value)
{
    CONFIG.mpu6050.hardwareLPF = *(BYTE*)value;
    mpu6050Reset();
}

void mpu6050GyroRangeChanged(const void * ref, const void * value)
{
    CONFIG.mpu6050.hardwareGyroRange = (*(BYTE*)value) & 0x02;
    mpu6050Reset();
}

void mpu6050AccRangeChanged(const void * ref, const void * value)
{
    CONFIG.mpu6050.hardwareAccRange = (*(BYTE*)value) & 0x02;
    mpu6050Reset();
}

void mpu6050Reset()
{
    STATE.mpu6050.connected = 0;
    STATE.mpu6050.gyroBufferFilled = 0;
    STATE.mpu6050.accBufferFilled = 0;
    STATE.mpu6050.bufferAccPosition = 0;
    STATE.mpu6050.bufferGyroPosition = 0;

    while (!mpu6050Init())
    {
        vTaskDelay(10);
    }
    
    i2cQueueAction(&mpu);

    STATE.mpu6050.connected = 1;
}

/**
 * Initializes all the necessary mpu registers
 *
 * @return BOOL true if succeeded
 */
BOOL mpu6050Init()
{
    IEC0CLR = _IEC0_INT2IE_MASK;

	//ConfigINT2(EXT_INT_DISABLE | RISING_EDGE_INT | EXT_INT_PRI_5);
    if (!mpu6050Check()) return FALSE;

    if (!mpu6050SetPowerManagement(0x80)) return FALSE;
    vTaskDelay(configTICK_RATE_HZ / 50);
    if (!mpu6050SetPowerManagement(0x02)) return FALSE;
    if (!mpu6050SetRate()) return FALSE;
    if (!mpu6050SetSensorScales()) return FALSE;
    if (!mpu6050SetInterrupts()) return FALSE;

    INTCONSET = _INTCON_INT2EP_MASK;
    IEC0SET = _IEC0_INT2IE_MASK;
    IPC2bits.INT2IP = 5;
    //ConfigINT2(EXT_INT_ENABLE | RISING_EDGE_INT | EXT_INT_PRI_5);

    return TRUE;
}

/**
 * Called by driverSensors -> sensorInit
 *
 * !! Will block if mpu is not detected !!
 */
void mpu6050Setup()
{
    mpuReadRegister = 0x3B;
    mpu.address = MPU6050_ADDRESS;
    mpu.writeLength = 1;
    mpu.writePacket = &mpuReadRegister;

    mpu.readLength = 14;
    mpu.readPacket = mpuBuffer;

    mpu.callbackFunction = &mpu6050Callback;
    mpu.state = I2C_STATE_DONE;

    while (!mpu6050Init())
    {
        BUZZ(BUZZ_DELAY_ERROR, 1);
        BUZZ(BUZZ_DELAY_NOTIFY, 4);

        vTaskDelay(configTICK_RATE_HZ);
    }

    STATE.mpu6050.connected = 1;
}

void __attribute__((vector(_EXTERNAL_2_VECTOR), interrupt(ipl5))) ISRWRAPPER_INT2();

/**
 * INT2 > Data ready interrupt
 */
void ISR_INT2()
{
    IFS0CLR = _IFS0_INT2IF_MASK;
    //INTClearFlag(INT_INT2);

    if (STATE.mpu6050.connected)
    {
        if (mpu.state >= I2C_STATE_DONE)
        {
            i2cQueueActionFromISR(&mpu);
        }
    }

    portEND_SWITCHING_ISR(NULL);
}
